#include <boost/static_assert.hpp>
#include <boost/mpl/int.hpp>
#include <boost/foreach.hpp>
#include <boost/type_traits.hpp>

#include <GL/glut.h>
#include <unistd.h>
#include <string>




#include "config.h"
#include "matrix.h"
#include "primitive.h"
#include "integer.h"
#include "predicate_2d.h"
#include "predicate_3d.h"
#include "intersect_2d.h"
#include "intersect_3d.h"
#include "mesh.h"
#include "hierarchy_mesh.h"


#include "aabbtree.h"
#include "msd.h"


typedef math::msd_mesh_traits <double, 3>::FaceType            face_type ;
#define GLFLOAT(VAL) VAL
typedef double value_type; 

typedef math::MSDMesh<math::msd_mesh_traits<double, 3> >         mesh_type ;
typedef math::HierarchyMesh<math::mesh_traits <double, 3> >      hierarchy_mesh_type ;
typedef math::AABBTreePoly <value_type, 3>                       AABBTreePolyType; 
typedef math::Sphere<value_type, 3>                              sphere_type ;
typedef math::Polygon<value_type, 3>                             polygon_type ;
typedef math::vector<value_type, 3>                              point3 ;
typedef math::vector<value_type, 2>                              point2 ;
typedef math::vector<value_type, 3>                              vector3 ;
typedef math::vector<value_type, 2>                              vector2 ;


unsigned integration_method = 2 ;

value_type off_x = 0.0;

int WINDOW_WIDTH = 900, WINDOW_HEIGHT = 600;

// mouse state
int prevX = 0, prevY = 0;
bool leftPressed = false, rightPressed = false, middlePressed = false;

// view state
float rotMat [ 16 ] = {1,0,0,0,
					   0,1,0,0,
					   0,0,1,0,
					   0,0,0,1};
float focus [ 3 ] = {0,0,-3.5};

bool gb_light = false ; 
bool gb_start = false ;

mesh_type mesh; 
hierarchy_mesh_type model ;
sphere_type sphere (point3 (0, 0, 0), 0.25 ); 

void reshape(int width, int height)
{
	glViewport(0, 0, width, height);
	WINDOW_WIDTH = width;
	WINDOW_HEIGHT = height;
}

void init_light(void)
{
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);
	glEnable(GL_NORMALIZE);

	// Light model parameters:
	// -------------------------------------------

	GLfloat lmKa[] = {0.0, 0.0, 0.0, 0.0 };
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT, lmKa);

	glLightModelf(GL_LIGHT_MODEL_LOCAL_VIEWER, 1.0);
	glLightModelf(GL_LIGHT_MODEL_TWO_SIDE, 0.0);

	// -------------------------------------------
	// Spotlight Attenuation

	GLfloat spot_direction[] = {1.0, -1.0, -1.0 };
	GLint spot_exponent = 30;
	GLint spot_cutoff = 180;

	glLightfv(GL_LIGHT0, GL_SPOT_DIRECTION, spot_direction);
	glLighti(GL_LIGHT0, GL_SPOT_EXPONENT, spot_exponent);
	glLighti(GL_LIGHT0, GL_SPOT_CUTOFF, spot_cutoff);

	GLfloat Kc = 1.0;
	GLfloat Kl = 0.0;
	GLfloat Kq = 0.0;

	glLightf(GL_LIGHT0, GL_CONSTANT_ATTENUATION,Kc);
	glLightf(GL_LIGHT0, GL_LINEAR_ATTENUATION, Kl);
	glLightf(GL_LIGHT0, GL_QUADRATIC_ATTENUATION, Kq);


	// ------------------------------------------- 
	// Lighting parameters:

	GLfloat light_pos[] = {0.0f, 5.0f, 5.0f, 1.0f};
                
	GLfloat light_Ka[]  = {0.8f, .8f, .8f, 1.0f};
	GLfloat light_Kd[]  = {.5f, .3f, 0.9f, 1.0f};
	GLfloat light_Ks[]  = {.8f, .8f, .8f, 1.0f};

	//GLfloat light_Ka[]  = {1.0f, 0.5f, 0.5f, 1.0f};
	//GLfloat light_Kd[]  = {1.f, 0.1f, 0.1f, 1.0f};
	//GLfloat light_Ks[]  = {1.0f, 1.0f, 1.0f, 1.0f};

	glLightfv(GL_LIGHT0, GL_POSITION, light_pos);
	glLightfv(GL_LIGHT0, GL_AMBIENT, light_Ka);
	glLightfv(GL_LIGHT0, GL_DIFFUSE, light_Kd);
	glLightfv(GL_LIGHT0, GL_SPECULAR, light_Ks);

	// -------------------------------------------
	// Material parameters:

	//GLfloat material_Ka[] = {0.5f, 0.0f, 0.0f, 1.0f};
	//GLfloat material_Kd[] = {0.4f, 0.4f, 0.5f, 1.0f};
	//GLfloat material_Ks[] = {0.8f, 0.8f, 0.0f, 1.0f};
	GLfloat material_Ka[] = {.5f, .5f, .5f, 1.0f};
	GLfloat material_Kd[] = {0.4f, 0.4f, 0.4f, 1.0f};
	GLfloat material_Ks[] = {0.8f, 0.8f, 0.8f, 1.0f};
	GLfloat material_Ke[] = {0.1f, 0.1f, 0.1f, 0.0f};
	GLfloat material_Se = 20.0f;

	glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, material_Ka);
	glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, material_Kd);
	glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, material_Ks);
	glMaterialfv(GL_FRONT_AND_BACK, GL_EMISSION, material_Ke);
	glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, material_Se);

}


void init(void)
{
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glViewport(0, 0, WINDOW_WIDTH, WINDOW_HEIGHT);

	glClearColor(0, 0, 0, 1);   

	GLfloat mat_specular[] = { 1.0, 1.0, 1.0, 1.0 };
	GLfloat mat_shininess[] = { 20.0 };
//	GLfloat light_position[] = { -5.0, 5.0, 5.0, 0.0 };
	glClearColor (0.0, 0.0, 0.0, 0.0);
	glShadeModel (GL_SMOOTH);

	init_light();
/*
	glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
	glMaterialfv(GL_FRONT, GL_SHININESS, mat_shininess);
	glLightfv(GL_LIGHT0, GL_POSITION, light_position);
*/
}

void draw_triangles_smooth (const std::vector<point3 *> & point, const std::vector<vector3 *> & normal, int faceid	,	std::vector<int> pv)
{
	point3 p (0.0f, 0.0f, 0.0f);
	glBegin(GL_POLYGON);
	
	for (unsigned i = 0; i < point.size(); ++i)
	{
		p += (*point[i]) ;
		glNormal3f(GLFLOAT((*normal[i])[0]), GLFLOAT((*normal[i])[1]), GLFLOAT((*normal[i])[2]));
		glVertex3f( GLFLOAT((*point[i])[0]), GLFLOAT((*point[i])[1]), GLFLOAT((*point[i])[2]));
	}

	p /= value_type(point.size());
	glEnd();
/*
	if (gb_number)
	{
		draw_number (p[0], p[1], p[2], faceid);
	}

	for (unsigned i = 0; i < point.size(); ++i)
	{
		if (gb_vertex)
		{
			draw_number (GLFLOAT((*point[i])[0]), GLFLOAT((*point[i])[1]), GLFLOAT((*point[i])[2]), pv[i]);
		}
	}
*/
}


void draw_triangles_facet (const std::vector<point3 *> & point, const vector3 & normal, int faceid	,	std::vector<int> pv, double ratio)
{
	point3 p (0.0f, 0.0f, 0.0f);
	glBegin(GL_POLYGON);
	glNormal3f(GLFLOAT(normal[0]), GLFLOAT(normal[1]), GLFLOAT(normal[2])); 
	for (unsigned i = 0; i < point.size(); ++i)
	{
		p += (*point[i]) ;
		glVertex3f( GLFLOAT((*point[i])[0]) * ratio, GLFLOAT((*point[i])[1]) * ratio, GLFLOAT((*point[i])[2]) * ratio);
	}

	p /= value_type(point.size());
	glEnd();
/*
	if (gb_number)
	{
		draw_number (p[0], p[1], p[2], faceid);
	}

	for (unsigned i = 0; i < point.size(); ++i)
	{
		if (gb_vertex)
		{
			draw_number (GLFLOAT((*point[i])[0]), GLFLOAT((*point[i])[1]), GLFLOAT((*point[i])[2]), pv[i]);
		}
	}
*/
}

void draw_mesh_smooth (mesh_type * mesh)
{
	GLfloat material_Ka[] = {.5f, .0f, .0f, 1.0f};
	glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, material_Ka);

	for (unsigned i = 0; i < mesh->faces.size(); ++i)
	{
		std::vector<int> v_id ;
		mesh->faces[i]->Face2VertexIdx(v_id); 
		
		
		std::vector<point3 *> p;
		std::vector<vector3 *>normal ;
		for (unsigned j = 0; j < v_id.size(); ++j)
		{
			p.push_back(&mesh->vertices[v_id[j]]->m_coord) ;
			normal.push_back(&mesh->vertices[v_id[j]]->get_normal());
		}
		draw_triangles_smooth ( p, normal, i, v_id); 

	}	
}
 
void draw_mesh_facet (hierarchy_mesh_type * mesh)
{
	GLfloat material_Ka[] = {.5f, .5f, .5f, 1.0f};
	glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, material_Ka);

	for (unsigned i = 0; i < mesh->faces.size(); ++i)
	{
		std::vector<point3 *> p;
		mesh->faces[i]->Face2VertexCoord(p); 
		std::vector<int> pv;
		mesh->faces[i]->Face2VertexIdx(pv);
		/*
		if (mesh->vertices[pv[0]]->m_intersect == true ||
			mesh->vertices[pv[1]]->m_intersect == true ||
			mesh->vertices[pv[2]]->m_intersect == true )
		{
			glColor3f (1.0, 0.0, 0.0);
		}
		*/
		draw_triangles_facet ( p, mesh->faces[i]->normal(), i, pv, 0.97); 
		{
			glColor3f (1.0, 1.0, 1.0);
		}
	}	
}

std::vector<unsigned > v_face_id; 
/*
bool intersec_detect(const vector3 & center, const value_type & radius )
{
	v_face_id.clear(); 
	sphere_type s (center, radius);
	return mesh.intersect (s, v_face_id);
}
*/

void draw_sphere (sphere_type & sphere)
{

	//const vector3 & center = sphere.get_center() ;


	vector3 center (0.0, 0.0, -1.0);
	value_type radius = sphere.get_radius() ;
	glTranslatef(center.x, center.y, center.z) ;
	if (gb_light) 
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	else 
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	glutSolidSphere( radius , 20, 20);
	glTranslatef(-center.x, -center.y, -center.z) ;

}
void draw_intersect_face (const mesh_type & mesh, const std::vector<unsigned> & v_face_id)
{
	glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	glColor3f (1.0, 0, 0);
	glLineWidth(4.0);
	for (unsigned i = 0; i < v_face_id.size(); ++i)
	{
		std::vector<point3 *> p;
		mesh.faces[v_face_id[i]]->Face2VertexCoord(p); 
		std::vector<int> pv;
		mesh.faces[v_face_id[i]]->Face2VertexIdx(pv);
		draw_triangles_facet ( p, mesh.faces[i]->normal(), i, pv, 1.0); 
	}		
	glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	glColor3f (1.0, 1.0, 1.0);
	glLineWidth(1.0);
}
void display() 
{
	if (gb_light)
	{
		glEnable(GL_LIGHT0);
		glEnable(GL_LIGHTING);
		glEnable(GL_DEPTH_TEST);	
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
		//glEnable(GL_CULL_FACE);

	}
	else
	{
		glDisable(GL_LIGHT0);
		glDisable(GL_LIGHTING);
		glDisable(GL_CULL_FACE);
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

	}

	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);

	// Set up viewing matrices
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60, float(WINDOW_WIDTH)/WINDOW_HEIGHT, .0001, 100);
	glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
	glPushMatrix();


	//Camera


	glTranslatef(focus[0], focus[1], focus[2]);

	glMultMatrixf(rotMat);

	glRotatef(-90, 1, 0, 0);

	// drawing code goes here
	//glutSolidCube ( 1 );
//	draw_sphere(sphere); 
//	draw_model(model);
	draw_mesh_smooth (& mesh);
	draw_mesh_facet (& model);
//	draw_mesh_facet (& mesh);
//	draw_intersect_face(mesh, v_face_id); 

	glPopMatrix();
	glFlush ();
	glutSwapBuffers();
}


void mouse(int button, int state, int x, int y)
{
	y = WINDOW_HEIGHT - y;

	// Mouse state that should always be stored on pressing
	if (state == GLUT_DOWN)
	{
		prevX = x;
		prevY = y;
	}

	if (button == GLUT_LEFT_BUTTON)
	{
		leftPressed = state == GLUT_DOWN;
	}

	if (button == GLUT_RIGHT_BUTTON)
	{
		rightPressed = state == GLUT_DOWN;
	}
	
	if (button == GLUT_MIDDLE_BUTTON)
	{
		middlePressed = state == GLUT_DOWN;
	}
}


//math::pbm::system <math::pbm::bounce_ball> bounce; 
//math::pbm::system_state <math::vector <double, 3>, math::pbm::bounce_ball> initial_state;

void idle()
{
	/*
	bounce.run();
	sphere.set_center (bounce.get_state().position);
	*/
	//mesh.update_model_eular();
	if (gb_start) mesh.update_model(integration_method);
	glutPostRedisplay(); 
}
void motion(int x, int y)
{
	y = WINDOW_HEIGHT - y;

	float dx = (x - prevX);
	float dy = (y - prevY);

	// rotate the scene
	if (leftPressed)
	{
		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
		glRotatef(dx, 0, 1, 0);
		glRotatef(dy, -1, 0, 0);
		glMultMatrixf(rotMat);
		glGetFloatv(GL_MODELVIEW_MATRIX, rotMat);
	}
	else if (middlePressed)
	{
		focus [ 0 ] += 0.005 * dx;
		focus [ 1 ] += 0.005 * dy;
	}
	else if (rightPressed)
	{
		focus [ 2 ] += 0.01 * dy;
	}
	
	// Store previous mouse positions
	prevX = x;
	prevY = y;

	glutPostRedisplay();
}


void keyboard(unsigned char key, int x, int y)
{
	//if (key == 'f' || key == 'F') frame = !frame ; 
	//if (key == 'b' || key == 'B') gb_number = !gb_number ;
	//if (key == 'v' || key == 'V') gb_vertex = !gb_vertex; 
	if (key == 'l' || key == 'L') gb_light = !gb_light; 
	if (key == 's' || key == 'S') gb_start = !gb_start; 
	glutPostRedisplay();
}


int main(int argc, char * argv[])
{
	try
	{
		mesh.set_model(&model);
		std::string mesh_filename , model_filename;
		
		std::cin>>mesh_filename; 
		std::cin>>model_filename ;
		std::cin>>integration_method ;

		std::ifstream ifs (mesh_filename);
		std::ifstream ifs_model (model_filename);
		ifs>>mesh; 
		ifs_model >> model ;
		mesh.translate (vector3 (0.0, 0.0, 2.0));
		model.scale(0.5);
		model.rotateX(1.5707963);
		model.build_hierarchy();

/*
		mesh.vertices[4]->lock(); // lock vertices
		mesh.vertices[3]->lock(); // lock vertices
		mesh.vertices[0]->lock(); // lock vertices
		mesh.vertices[7]->lock(); // lock vertices 
*/
/*

		mesh.vertices[1]->lock(); // lock vertices
		mesh.vertices[2]->lock(); // lock vertices
		mesh.vertices[5]->lock(); // lock vertices
		mesh.vertices[6]->lock(); // lock vertices 
*/

//		mesh.vertices[0]->lock();
//		mesh.vertices[1]->lock(); 


		for (mesh_type::VertexIterator viter = mesh.vertices.begin(); viter != mesh.vertices.end(); ++viter)
		{
			
			
			if ( std::abs(viter->second->m_coord.x - 1.0) < 1e-5)
			{
				viter->second->lock();
				continue; 
			}			
		}

//		mesh.rotateX (1.5707); 
//		mesh.build_hierarchy(); 



//		bounce.initialize(initial_state, time_step);

//		bounce.set_object(mesh, sphere);

	}
	catch (const char errormsg)
	{
		std::cout<<errormsg<<std::endl;
	}

	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
	glutInitWindowPosition(0, 0);
	glutInitWindowSize(WINDOW_WIDTH, WINDOW_HEIGHT);
	glutCreateWindow("Bounce Ball Simulation.");

	init();

	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	glutIdleFunc(idle);

	glutKeyboardFunc(keyboard);
	glutMouseFunc(mouse);
	glutMotionFunc(motion);

	// main loop
	glutMainLoop();
	return 0;
}
